#include "XBEES2Lib.h"

//	XBee Object
XBee	_xbee = XBee();
//	Buffer for last data received from XBee
uint8_t	XBeeS2_lastDataReceived[MAX_PAYLOAD];
//	Lenght of the buffer
uint8_t XBeeS2_lenghtDataReceived;

bool	_xbee_busy = false;
bool	_xbee_module_started = false;
bool	_not_joined_to_network = false;


char XBeeS2_Start( ){
	#ifdef DEBUG_XBEE
		Serial.print("\tStarted XBee S2: { ");
	#endif

	_xbee.setSerial(Serial3);

	#ifdef DEBUG_XBEE
		Serial.println(" }");
	#endif

	_xbee_busy = false;
	_xbee_module_started = false;
	_not_joined_to_network = false;

	return 1;
}

void XBeeS2_ResetSendStatus (){
	_xbee_busy = false;
	_xbee_module_started = false;
	_not_joined_to_network = false;
}

bool XBeeS2_ModemStatusResponse (){
	XBeeResponse response = XBeeResponse();
	ModemStatusResponse mr = ModemStatusResponse();
	//ZBRxResponse rx = ZBRxResponse();

	_xbee.readPacket();
    
    if (_xbee.getResponse().isAvailable()) {
		//	Got data
		if (_xbee.getResponse().getApiId() == MODEM_STATUS_RESPONSE) {
			//	Getting data
			_xbee.getResponse().getModemStatusResponse(mr);
			#ifdef DEBUG_XBEE
				Serial.print ("\t<<< Modem Status Response OK Xbee S2 {");
				
				//Serial.println (mr, HEX);
				Serial.println (mr.getStatus(), HEX);
			#endif
			
		}
    }
	return false;
}

bool XBeeS2_SendDataToAddrXBee (uint32_t _msb, uint32_t _lsb, uint8_t* _data, uint8_t _size){
	XBeeAddress64 sendAddr = XBeeAddress64(_msb, _lsb);
	ZBTxRequest zbTx = ZBTxRequest(sendAddr, _data, sizeof(uint8_t)*_size);
	ZBTxStatusResponse txStatus = ZBTxStatusResponse();
	
	#ifdef DEBUG_XBEE
			/*
			Serial.print ("\t<<< Status Xbee S2 { ");
			_xbee_busy == true ? Serial.print("BUSY") : Serial.print("IDLE");
			Serial.println (" }");
			*/
	#endif

	if (!_xbee_busy && _xbee_module_started){
		#ifdef DEBUG_XBEE
			Serial.println ("\t\t++ Sending Data Xbee S2");
		#endif
		_xbee.send(zbTx);
		_xbee_busy = true;
		_not_joined_to_network = false;
	}

	_xbee_module_started = true;

	//	After sending a packet, a response should be available within 500 ms
	if (_xbee.readPacket(900)){
		//	Got a response
		//	Should be a znet tx status 
		if (_xbee.getResponse().getApiId() == ZB_TX_STATUS_RESPONSE){
			_xbee.getResponse().getZBTxStatusResponse(txStatus);
			// get the delivery status, the fifth byte
			if (txStatus.getDeliveryStatus() == SUCCESS) {
				#ifdef DEBUG_XBEE
					Serial.println ("\t<<< Data sent OK Xbee S2");
				#endif
				_xbee_busy = false;
				return true;
			} else {
				#ifdef DEBUG_XBEE
					Serial.print ("\t<<< Error Delivery Status Xbee S2 { ");
					switch (txStatus.getDeliveryStatus()){
						case NOT_JOINED_TO_NETWORK:
							Serial.print ("NOT JOINED TO NETWORK");
							break;
						default:
							Serial.print ("NOT DEFINED");
							break;
					}
					Serial.println (" }");
				#endif
				_not_joined_to_network = true;
				_xbee_module_started = false;
				return false;
			}
		}
	} else if (_xbee.getResponse().isError()) {
		#ifdef DEBUG_XBEE
			if (_xbee.getResponse().getErrorCode() != UNEXPECTED_START_BYTE){
				Serial.print ("\t<<< Error Response Xbee S2 { ");
				Serial.print (_xbee.getResponse().getErrorCode(), HEX);
				Serial.println (" }");
			}
			else{
				Serial3.flush();
				//	TRASH DATA
			}
		#endif
		return false;
	} else {
		#ifdef DEBUG_XBEE
			//Serial.println ("\t<<< Module Xbee S2 Error (UNLIKELY): { MODULE FAIL, TIMING TX }");
		#endif
		if (_not_joined_to_network){
			_xbee_busy = false;
			_xbee_module_started = false;
		}
		return false;
	}
	return false;
}

bool XBeeS2_ReceiveDataFromXbee (){
	XBeeResponse response = XBeeResponse();
	ZBRxResponse rx = ZBRxResponse();

	_xbee.readPacket();
    
    if (_xbee.getResponse().isAvailable()) {
		//	Got data
		if (_xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
			//	Getting data
			_xbee.getResponse().getZBRxResponse(rx);
			memset(XBeeS2_lastDataReceived, 0, MAX_PAYLOAD);
			XBeeS2_lenghtDataReceived = rx.getDataLength();

			for (int i = 0 ; i < rx.getDataLength() ; i ++){
				XBeeS2_lastDataReceived[i] = rx.getData(i);
			}

			#ifdef DEBUG_XBEE
				Serial.println ("\t<<< Data received OK Xbee S2");
			#endif
			if (_xbee.getResponse().getApiId() == ZB_RX_RESPONSE)
				return true;
			else
				return false;
		} else if (_xbee.getResponse().isError()) {
			XBeeS2_lenghtDataReceived = 0;
			#ifdef DEBUG_XBEE
				Serial.print ("\t<<< Error receiving data Xbee S2 { ");
				Serial.print (_xbee.getResponse().getErrorCode(), HEX);
				Serial.println (" }");
			#endif
			return false;
		}
    }
	return false;
}

bool XBeeS2_SendArrayToAddrXBee (uint32_t _msb, uint32_t _lsb, uint8_t* _data, uint16_t _size, uint8_t _cmd ){
	uint16_t	n_sends	= _size / MAX_PAYLOAD;
	uint16_t n_samples = _size;
	uint16_t cont = 0;

	if (n_samples > MAX_PAYLOAD){
		#ifdef DEBUG_XBEE
			Serial.print ("\t<<< Sending array XBee S2. Sents: ");
			Serial.println (n_sends);
		#endif
		for (int i = 0 ; i < n_sends ; i ++){

			#ifdef DEBUG_XBEE
				Serial.print ("\t<<< Send: ");
				Serial.println (i+1);
			#endif

			if (n_samples > MAX_PAYLOAD){
				_data[cont] = _cmd;
				while((!XBeeS2_SendDataToAddrXBee(_msb, _lsb, &_data[cont], MAX_PAYLOAD))&&(!_sleep_interrupt)){
					delay(500);
				}

				#ifdef DEBUG_XBEE
					Serial.println ("\t<<< Waiting for continue cmd XBee S2");
				#endif
				while ((!XBeeS2_ReceiveDataFromXbee())&&(!_sleep_interrupt)){}
				if (XBeeS2_lastDataReceived[0] == CONTINUE){
					n_samples -= MAX_PAYLOAD;
					cont += MAX_PAYLOAD;
				}
			}
			else{
				while((!XBeeS2_SendDataToAddrXBee(_msb, _lsb, &_data[cont], n_samples))&&
					  (!_sleep_interrupt)){
				}
				#ifdef DEBUG_XBEE
					Serial.println ("\t<<< Waiting for continue cmd XBee S2");
				#endif
				while ((!XBeeS2_ReceiveDataFromXbee())&&(!_sleep_interrupt)){}
			}
		}
	}
	else{
		#ifdef DEBUG_XBEE
			Serial.println ("\t<<< Sending array XBee S2. Unique");
		#endif
		_data[cont] = _cmd;
		while((!XBeeS2_SendDataToAddrXBee (_msb, _lsb, _data, _size))&&
			  (!_sleep_interrupt)){
		}
		while ((!XBeeS2_ReceiveDataFromXbee())&&(!_sleep_interrupt)){}
	}

	if (_sleep_interrupt)
		return false;
	else
		return true;
}

void XBeeS2_ResetDataReceived ( void ){
	memset(XBeeS2_lastDataReceived, 0, MAX_PAYLOAD);
}

bool XBeeS2_WaitContinue ( void ){
	while (!XBeeS2_ReceiveDataFromXbee()){
		switch (XBeeS2_lastDataReceived[0]){
			case CONTINUE:
				return true;	
			default:
				return false;
		}
	}
	return false;
}

bool XBeeS2_CheckXBee (){
	uint8_t atsm_cmd[]={'V','R'};
	AtCommandRequest _ATcmd = AtCommandRequest(atsm_cmd);
	AtCommandResponse _ATrsp = AtCommandResponse();

	_xbee.send(_ATcmd);

	//	After sending a packet, a response should be available within 500 ms
	if (_xbee.readPacket(500)){
		//	Got a response
		//	Should be a znet tx status 
		if (_xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE){
			_xbee.getResponse().getAtCommandResponse(_ATrsp);
			// get the delivery status, the fifth byte
			if (_ATrsp.isOk()) {
				#ifdef DEBUG_XBEE
					Serial.println ("\tModule Xbee S2: OK");
				#endif
				return true;
			} else {
				#ifdef DEBUG_XBEE
					Serial.println ("\tModule Xbee S2: FAIL");
				#endif
				return false;
			}
		}
		else{
			#ifdef DEBUG_XBEE
				Serial.println ("\tModule Xbee S2: FAIL");
			#endif
			return false;
		}
    }
	else {
		#ifdef DEBUG_XBEE
			Serial.println ("\tModule Xbee S2: FAIL");
		#endif
		return false;
	}
}

